#include "equalizergroup.h"
#include <math.h>

EqualizersGroup::EqualizersGroup(QObject *parent) : QObject(parent)
{

}

EqualizersGroup::~EqualizersGroup()
{
    clear();
}


void EqualizersGroup::addFilter(FilterType type, double cf, double gainDB, double q)
{
    EQ_Info *info = new EQ_Info;

    info->type = type;
    info->cf = cf;
    info->cf_max = freqRespEndFreq;
    info->cf_min = freqRespStartFreq;
    info->gain = gainDB;
    info->gain_max = 24;
    info->gain_max = -24;
    info->q = q;
    info->q_max = 20;
    info->q_min = 0.05;
    eqInfo.append(info);
}

int EqualizersGroup::getFilterCount()
{
    return eqInfo.size();
}

void EqualizersGroup::setEqualizerGroupSampleRate(double sample)
{
    sampleRate = sample;
}

void EqualizersGroup::setFilterCenterFrequency(int num, double cf)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->cf = cf;
    }
}

void EqualizersGroup::setFilterCenterFrequencyMax(int num, double max)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->cf_max = max;
    }
}

void EqualizersGroup::setFilterCenterFrequencyMin(int num, double min)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->cf_min = min;
    }
}

void EqualizersGroup::setFilterGainDB(int num, double gainDB)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->gain = gainDB;
    }
}

void EqualizersGroup::setFilterGainDBMax(int num, double max)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->gain_max = max;
    }
}

void EqualizersGroup::setFilterGainDBMin(int num, double min)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->gain_min = min;
    }
}

void EqualizersGroup::setFilterQ(int num, double q)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->q = q;
    }
}

void EqualizersGroup::setFilterQMax(int num, double max)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->q_max = max;
    }
}

void EqualizersGroup::setFilterQMin(int num, double min)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->q_min = min;
    }
}

void EqualizersGroup::setFilterGroupStartFrequency(double sf)
{
    freqRespStartFreq = sf;
}

void EqualizersGroup::setFilterGroupEndFrequency(double ef)
{
    freqRespEndFreq = ef;
}

void EqualizersGroup::setFilterFrequencyResponseInterval(double iv)
{
    if(iv > 0)
    {
        freqRespInterval = iv;
    }
}

void EqualizersGroup::setFilterCenterFrequencyAll(double cf)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->cf = cf;
    }
}

void EqualizersGroup::setFilterCenterFrequencyMaxAll(double max)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->cf_max = max;
    }
}

void EqualizersGroup::setFilterCenterFrequencyMinAll(double min)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->cf_min = min;
    }
}

void EqualizersGroup::setFilterGainDBAll(double gainDB)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->gain = gainDB;
    }}

void EqualizersGroup::setFilterGainDBMaxAll(double max)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->gain_max = max;
    }
}

void EqualizersGroup::setFilterGainDBMinAll(double min)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->gain_min = min;
    }
}

void EqualizersGroup::setFilterQAll(double q)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->q = q;
    }
}

void EqualizersGroup::setFilterQMaxAll(double max)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->q_max = max;
    }
}

void EqualizersGroup::setFilterQMinAll(double min)
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        eqInfo[i]->q_min = min;
    }
}

void EqualizersGroup::setFilterType(int num,FilterType type)
{
    if(num < eqInfo.size())
    {
        eqInfo[num]->type = type;
    }
}

double EqualizersGroup::getEqualizerGroupSampleRate()
{
    return sampleRate;
}

int EqualizersGroup::getFilterType(int num)
{
    if(num < eqInfo.size())
    {
        return (int)eqInfo[num]->type;
    }
}

double EqualizersGroup::getFilterCenterFrequency(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->cf;
    }
    return 0;
}

double EqualizersGroup::getFilterCenterFrequencyMax(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->cf_max;
    }
    return 0;
}

double EqualizersGroup::getFilterCenterFrequencyMin(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->cf_min;
    }
    return 0;
}

double EqualizersGroup::getFilterGainDB(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->gain;
    }
    return 0;
}

double EqualizersGroup::getFilterGainDBMax(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->gain_max;
    }
    return 0;
}

double EqualizersGroup::getFilterGainDBMin(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->gain_min;
    }
    return 0;
}

double EqualizersGroup::getFilterQ(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->q;
    }
    return 0;
}

double EqualizersGroup::getFilterQMax(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->q_max;
    }
    return 0;
}

double EqualizersGroup::getFilterQMin(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->q_min;
    }
    return 0;
}

double EqualizersGroup::getFilterGroupStartFrequency()
{
    return freqRespStartFreq;
}

double EqualizersGroup::getFilterGroupEndFrequency()
{
    return freqRespEndFreq;
}

double EqualizersGroup::getFilterFrequencyResponseInterval()
{
    return freqRespInterval;
}

void EqualizersGroup::updateEqualizer(int num)
{
    if(num < eqInfo.size())
    {
        FilterType filterType = eqInfo[num]->type;
        double Q = eqInfo[num]->q;
        double gainDB = eqInfo[num]->gain;
        double centerFreq = eqInfo[num]->cf;
        Q = (Q == 0) ? 1e-9 : Q;
        double gain_abs = ::pow(10.0, gainDB / 40.0);
        double omega = 2.0f * pi * centerFreq / sampleRate;
        double sn = sin(omega);
        double cs = cos(omega);
        double alpha = sn / (2.0 * Q);
        double beta = sqrt(gain_abs + gain_abs);
        double a0,a1,a2,b0,b1,b2;

        switch (filterType) {
        case Bandpass:
            b0 = alpha;
            b1 = 0;
            b2 = -alpha;
            a0 = 1.0 + alpha;
            a1 = -2.0 * cs;
            a2 = 1.0 - alpha;
            break;
        case Lowpass:
            b0 = (1 - cs) / 2;
            b1 = 1 - cs;
            b2 = (1 - cs) / 2;
            a0 = 1 + alpha;
            a1 = -2 * cs;
            a2 = 1 - alpha;
            break;
        case Hightpass:
            b0 = (1 + cs) / 2;
            b1 = -(1 + cs);
            b2 = (1 + cs) / 2;
            a0 = 1 + alpha;
            a1 = -2 * cs;
            a2 = 1 - alpha;
            break;
        case Notch:
            b0 = 1;
            b1 = -2 * cs;
            b2 = 1;
            a0 = 1 + alpha;
            a1 = -2 * cs;
            a2 = 1 - alpha;
            break;
        case Peak:
            b0 = 1 + (alpha * gain_abs);
            b1 = -2 * cs;
            b2 = 1 - (alpha * gain_abs);
            a0 = 1 + (alpha / gain_abs);
            a1 = -2 * cs;
            a2 = 1 - (alpha / gain_abs);
            break;
        case Lowshelf:
            b0 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs + beta * sn);
            b1 = 2 * gain_abs * ((gain_abs - 1) - (gain_abs + 1) * cs);
            b2 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs - beta * sn);
            a0 = (gain_abs + 1) + (gain_abs - 1) * cs + beta * sn;
            a1 = -2 * ((gain_abs - 1) + (gain_abs + 1) * cs);
            a2 = (gain_abs + 1) + (gain_abs - 1) * cs - beta * sn;
            break;
        case Highshelf:
            b0 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs + beta * sn);
            b1 = -2 * gain_abs * ((gain_abs - 1) + (gain_abs + 1) * cs);
            b2 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs - beta * sn);
            a0 = (gain_abs + 1) - (gain_abs - 1) * cs + beta * sn;
            a1 = 2 * ((gain_abs - 1) - (gain_abs + 1) * cs);
            a2 = (gain_abs + 1) - (gain_abs - 1) * cs - beta * sn;
            break;
        }
        b0 = b0 / a0;
        b1 = b1 / a0;
        b2 = b2 / a0;
        a1 = a1 / a0;
        a2 = a2 / a0;

        eqInfo[num]->param.b0 = b0;
        eqInfo[num]->param.b1 = b1;
        eqInfo[num]->param.b2 = b2;
        eqInfo[num]->param.a2 = a0;
        eqInfo[num]->param.a1 = a1;

        eqInfo[num]->freqResp.clear();
        double sf = freqRespStartFreq;
        double ef = freqRespEndFreq;
        double fri = freqRespInterval;
        double phi;
        double r;
        double g;
        double dB;

        while(sf <= ef)
        {
            phi = ::pow((::sin(2.0 * pi * sf / (2.0 * sampleRate))), 2.0);
            r = (::pow(b0 + b1 + b2, 2.0) - 4.0 * (b0 * b1 + 4.0 * b0 * b2 + b1 * b2) * phi + 16.0 * b0 * b2 * phi * phi) / (::pow(1.0 + a1 + a2, 2.0) - 4.0 * (a1 + 4.0 * a2 + a1 * a2) * phi + 16.0 * a2 * phi * phi);
            if(r < 0) {
                r = 0;
            }
            g = ::sqrt(r);
            dB = 20 * log10(g == 0 ? 1 : g);
            eqInfo[num]->freqResp.append(dB);
            sf += fri;
        }
    }
}

QList<double> &EqualizersGroup::getFilterFrequencyResponse(int num)
{
    if(num < eqInfo.size())
    {
        return eqInfo[num]->freqResp;
    }
    static QList<double> null;
    null.clear();
    return null;
}

void EqualizersGroup::clear()
{
    for(int i=0; i<eqInfo.size(); i++)
    {
        delete eqInfo[i];
    }
    eqInfo.clear();
}
